/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"


/**
* Parameter demo
* Demonstrates the features of:
* - CMatchStorage
* - CEnvironmentMeasurement2D
**/
void parameterDemo(){
	CMatchStorage ms(12); 
	TPoseMatch pm;
	
	
	
	
	for(int i=0;i<100;i++){
		pm.weight = ((float) (rand()%10000))/10000; ///< [0...1]
		ms.update(pm);
	}
	
	for(int i=0;i<ms.storage.size();i++){
		fprintf(stderr,"%.3f ",ms.storage[i].weight);
	}
	ms.sort();
	fprintf(stderr,"\n");
	
	for(int i=0;i<ms.storage.size();i++){
		fprintf(stderr,"%.3f ",ms.storage[i].weight);
	}
	fprintf(stderr,"\n");
	fprintf(stderr,"Testing copying\n");
	
	char *data=NULL;
	unsigned int size = ms.getNetPacket(&data);
	CMatchStorage ms2(data,size);
	
	for(int i=0;i<ms.storage.size();i++){
		fprintf(stderr,"%.3f ",ms.storage[i].weight);
	}
	fprintf(stderr,"\n");
	for(int i=0;i<ms2.storage.size();i++){
		fprintf(stderr,"%.3f ",ms2.storage[i].weight);
	}
	fprintf(stderr,"\n");
	
	fprintf(stderr,"----------------------------------------------------------\n");
	fprintf(stderr,"Testing with EnvironmentMeasurement2D\n");
	CEnvironmentMeasurement2D meas;
	meas.frame.setFrame(1,2,3); 
	meas.frame.setError(4,5,6);
	
	for(int i=0;i<20;i++){
		TMeasurementPoint p(((float) (rand()%10000))/10000.0,((float) (rand()%10000))/10000.0);
		meas.addPoint(p);
	}
	char *ptr=NULL;
	unsigned int dsize = meas.getNetPacket(&ptr);
	FILE *outf = fopen("test.txt","wt");
	meas.fprint(outf);
	fclose(outf);
	FILE *infile = fopen("test.txt","rt");
	
	CEnvironmentMeasurement2D meas2(ptr,dsize);
	CEnvironmentMeasurement2D meas3(infile);
	meas.print();
	meas2.print();
	meas3.print();
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void testGUI(){
	fprintf(stderr,"----------------------------------------------------------\n");
	fprintf(stderr,"Testing GUI\n");
	COccuGUI ogui("../res/autmap-small.txt",0.05,0.5);
	ogui.drawGrid();
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void globalLocalizationDemo(){
	CEnvironmentMeasurement2D meas;
	COccuGUI ogui("../res/autmap-strip.txt",0.05,0.4);
	ogui.generateOccuMeasurement(meas,50);
	
	
	float x,y,a;
	x = meas.frame.x_ref; y = meas.frame.y_ref; a=meas.frame.a_ref;
	fprintf(stderr,"Got %d measurements from pose (%.3f,%.3f,%.3f) \n",meas.points.size(),meas.frame.x_ref,
					meas.frame.y_ref,meas.frame.a_ref );
	
	CBetBdtLocalizer localizer("../res/autmap-strip.txt",0.05);
	float search_x = 10;
	float search_y = 10;
	float search_a = 180.0*M_PI/180.0;
	meas.frame.setError(4*search_x,4*search_y,M_PI);
	
	meas.frame.x_ref +=search_x*((2.0*(rand()%10000))/10000.0-0.5);
	meas.frame.y_ref +=search_y*((2.0*(rand()%10000))/10000.0-0.5);
	meas.frame.a_ref +=search_a*((2.0*(rand()%10000))/10000.0-0.5);				 
	
	CMatchStorage matches(500);
	 
	localizer.localize(meas,matches);
	
	for(unsigned int i=0; i<matches.storage.size();i++){
			fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.3f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
							matches.storage[i].y,matches.storage[i].a,matches.storage[i].weight,matches.storage[i].ax,
							matches.storage[i].ay,matches.storage[i].aa);
	}
	
	fprintf(stderr,"Initial:(%.3f,%.3f,%.3f) true:(%.3f,%.3f,%.3f)\n",meas.frame.x_ref,
					meas.frame.y_ref,meas.frame.a_ref,x,y,a);
	
	ogui.plotMatchResults(matches,x,y,a);
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///Test SLAM
void MLSLAMDemo(){
	CEnvironmentMeasurement2D meas;	
	CBetBdtLocalizer slammer(100,0.05); 
	float x=0,y=0;
	int cnt = 0;
	
	COccuGUI sgui(100,0.05,0.5); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
	float search_x = 2.0;
	float search_y = 2.0;
	float search_a = 20.0*M_PI/180.0;
	CMatchStorage matches(10);
	bool run = true;
	COccuGUI ogui("../res/autmap-strip.txt",0.05,0.5);
	while(run){
			run = ogui.generateOccuMeasurement(meas,40.0);
			
			if(cnt == 0){
					x = meas.frame.x_ref; y = meas.frame.y_ref;
					meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0;
			}else{
					meas.frame.x_ref-=x;meas.frame.y_ref-=y;
					mx = meas.frame.x_ref; my = meas.frame.y_ref;ma=meas.frame.a_ref;
					
					meas.frame.x_ref +=search_x*((1.0*(rand()%10000))/10000.0-0.5);
					meas.frame.y_ref +=search_y*((1.0*(rand()%10000))/10000.0-0.5);
					meas.frame.a_ref +=search_a*((1.0*(rand()%10000))/10000.0-0.5);	
					meas.frame.setError(4*search_x,4*search_y,4*search_a);			 
			}
			cnt ++;
			//slammer.setMeasurementToMap(meas);
			//sgui.setOccupancyGrid(slammer.grid);
			//sgui.plotMatchResults(matches,meas.frame.x_ref,meas.frame.y_ref,0);
			
			
			if(slammer.localize(meas,matches)){
					fprintf(stderr,"Trueth:(%.2f,%.2f,%.2f) est:(%.2f,%.2f,%.2f)\n",
									mx,my,ma,meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
					
					for(unsigned int i=0; i<matches.storage.size();i++){
							fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.3f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
											matches.storage[i].y,matches.storage[i].a*180./M_PI ,matches.storage[i].weight,matches.storage[i].ax,
											matches.storage[i].ay,matches.storage[i].aa);
					}
					meas.frame.x_ref = matches.storage[0].x;
					meas.frame.y_ref = matches.storage[0].y;
					meas.frame.a_ref = matches.storage[0].a;
					
					slammer.setMeasurementToMap(meas);
					sgui.setOccupancyGrid(slammer.grid);
					if(run) run = sgui.plotMatchResults(matches,matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
			}else{
					sgui.setOccupancyGrid(slammer.grid);
					run = sgui.drawGrid();
			}
			
	}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///Test "Global" SLAM
void GlobalSLAMDemo(){
		CEnvironmentMeasurement2D meas;	
		CBetBdtLocalizer slammer(120,0.05); 
		float x=0,y=0;
		int cnt = 0;
	
		COccuGUI sgui(120,0.05,0.5); ///< for visualizing the SLAM map
		float mx=0,my=0,ma=0;
	
		float search_x = 80.0;
		float search_y = 80.0;
		float search_a = M_PI;
		CMatchStorage matches(500);
		bool run = true;
		COccuGUI ogui("../res/autmap-strip.txt",0.05,0.5);
		while(run){
				run = ogui.generateOccuMeasurement(meas,100.0);
			
				if(cnt == 0){
						x = meas.frame.x_ref; y = meas.frame.y_ref;
						meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0;
				}else{
						meas.frame.x_ref-=x;meas.frame.y_ref-=y;
						mx = meas.frame.x_ref; my = meas.frame.y_ref;ma=meas.frame.a_ref;
						meas.frame.x_ref = 0;meas.frame.y_ref = 0;meas.frame.a_ref = 0; ///No prior knowledge
						
						meas.frame.setError(search_x,search_y,search_a);			 
				}
				cnt ++;
			//slammer.setMeasurementToMap(meas);
			//sgui.setOccupancyGrid(slammer.grid);
			//sgui.plotMatchResults(matches,meas.frame.x_ref,meas.frame.y_ref,0);
			
			
				if(slammer.localize(meas,matches)){
						fprintf(stderr,"Trueth:(%.2f,%.2f,%.2f) est:(%.2f,%.2f,%.2f)\n",
										mx,my,ma,meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref);
					
						for(unsigned int i=0; i<matches.storage.size();i++){
								fprintf(stderr,"pose(%.3f,%.3f,%.3f) weight='%.3f' area(%.3f,%.3f,%.3f)\n",matches.storage[i].x,
												matches.storage[i].y,matches.storage[i].a*180./M_PI ,matches.storage[i].weight,matches.storage[i].ax,
												matches.storage[i].ay,matches.storage[i].aa);
						}
						meas.frame.x_ref = matches.storage[0].x;
						meas.frame.y_ref = matches.storage[0].y;
						meas.frame.a_ref = matches.storage[0].a;
					
						slammer.setMeasurementToMap(meas);
						sgui.setOccupancyGrid(slammer.grid);
						if(run) run = sgui.plotMatchResults(matches,matches.storage[0].x,matches.storage[0].y,matches.storage[0].a,0.25);
				}else{
						sgui.setOccupancyGrid(slammer.grid);
						run = sgui.drawGrid();
				}
			
		}
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void printUsage(){
		fprintf(stderr,"\n--------------------------------------------------------------------\n");
		fprintf(stderr,"Branch and Bound Localizer Demo Application\n");	
		fprintf(stderr,"\nThis application contains different use cases for BetB_dt Localizer\n");
		fprintf(stderr,"The different cases uses 'simulated' data which is requested as user input.\n");
		fprintf(stderr,"\nUsgage ./bbdt [num]\n");
		fprintf(stderr,"[num] a demo number:\n");
		fprintf(stderr,"0: Parameter demo\n");
		fprintf(stderr,"1: GUI demo\n");
		fprintf(stderr,"2: Global Localizer demo\n");
		fprintf(stderr,"3: ML-SLAM Demo\n");
		fprintf(stderr,"4: Global SLAM demo\n");
		fprintf(stderr,"--------------------------------------------------------------------\n\n\n");
}


int main(int argc, char *argv[]){
	  int demo = 0;
		srand(time(NULL));
		printUsage();
		if(argc != 2){
				fprintf(stderr,"No demo number given. Using default = '0'\n");
		}
		else{
				demo = atoi(argv[1]);
		}
		switch(demo){
				case 0:
						fprintf(stderr,"Running paramer demo\n");
						parameterDemo(); 
						break;
				case 1:
						fprintf(stderr,"Running GUI demo\n");
						testGUI(); 
						break;
				case 2:
						fprintf(stderr,"Running Global localizer demo\n"); 
						globalLocalizationDemo();
						break;
				case 3:
						fprintf(stderr,"Running ML-SLAM Demo\n"); 
						MLSLAMDemo();
						break;
				case 4:
						fprintf(stderr,"Running Global SLAM demo\n"); 
						GlobalSLAMDemo();
						break;
				default:
						fprintf(stderr,"Invalid input parameter\n"); 
						break;
		}
	
	fprintf(stderr,"Exited\n");	
	return 0;
}